package ca.usherbrooke.application.astar;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;
import java.util.Stack;

import ca.usherbrooke.application.astar.action.AStarMoveAction;
import ca.usherbrooke.application.astar.goal.Goal;
import ca.usherbrooke.application.astar.goal.GoalMove;
import ca.usherbrooke.application.astar.heuristique.Heuristique;
import ca.usherbrooke.application.astar.location.ILocation;

public class AStar<E extends Goal> {
	private static final Boolean DEBUG = false;

	public static final long INFINI_TIME = -1;

	public List<AStarMoveAction> generatePlan(AStarWorld world,
			ILocation startLocation, Heuristique<E> heuristique, E goal) {
		return generatePlan(world, startLocation, heuristique, goal,
				INFINI_TIME);
	}

	public List<AStarMoveAction> generatePlan(AStarWorld world,
			ILocation startLocation, Heuristique<E> heuristique, E goal,
			long timeInMs) {
		final long begin = System.currentTimeMillis();
		boolean noResult = false;

		Etat etatActuel = new Etat(world, null, null, startLocation);

		List<Successeur> open = new ArrayList<Successeur>();
		ArrayList<Etat> close = new ArrayList<Etat>();

		long current;
		do {
			// //////////DEBUG////////////////
			if (DEBUG) {
				/*
				DebugInfoPrint.INSTANCE.addCurrentState(etatActuel
						.getLocation().getPosition());
				DebugInfoPrint.INSTANCE.setCurrentPosition(etatActuel
						.getLocation().getPosition());

				List<Vector2D> allRoads = new ArrayList<Vector2D>();
				for (Road road : etatActuel.getLocation().getRoads()) {
					allRoads.add(road.getDestination().getPosition());
				}
				DebugInfoPrint.INSTANCE.setAllRoads(allRoads);
				*/
			}
			// //////////////////////////////

			Collection<Successeur> newSuccesseur = etatActuel
					.generateSuccessors();

			Iterator<Successeur> itt = newSuccesseur.iterator();
			while (itt.hasNext()) {
				Successeur successeur = itt.next();

				int idx = close.indexOf(successeur.getEtat());
				if (idx != -1) {
					// if (close.get(idx).getF() < successeur.getEtat().getF())
					// {
					itt.remove();
					// }
				} else if (open.contains(successeur)) {
					itt.remove();
				} else {
					Etat next = successeur.getEtat();
					next.setG(next.getParent().getG()
							+ (successeur.getCout() / 32.0f));// next.getParent().getG()
					// +
					next.setH((heuristique.estimeCoutRestant(next, goal)) / 32);
					next.setF(next.getG() + next.getH());
				}
			}
			open.addAll(newSuccesseur);

			// //////////DEBUG////////////////
			if (DEBUG) {
				/*
				List<Vector2D> openSpacePos = new ArrayList<Vector2D>();
				for (Successeur successeur : open) {
					openSpacePos.add(successeur.getEtat().getLocation()
							.getPosition());
				}
				DebugInfoPrint.INSTANCE.setOpenSpace(openSpacePos);
				*/
			}
			// //////////////////////////////

			if (!open.isEmpty()) {
				Successeur todo = open.get(0);
				double min = Double.MAX_VALUE;
				for (Successeur succ : open) {
					if (succ.getEtat().getF() < min) {
						todo = succ;
						min = succ.getEtat().getF();
					}
				}

				etatActuel = todo.getEtat();
				open.remove(todo);
				close.add(etatActuel);
			} else {
				noResult = true;
			}

			current = System.currentTimeMillis() - begin;
			if (timeInMs != INFINI_TIME && current > timeInMs) {
				break;
			}
		} while (!noResult && !goal.goalIsSatisfied(etatActuel));

		LinkedList<AStarMoveAction> plan = null;

		if (!noResult) {
			plan = new LinkedList<AStarMoveAction>();

			Stack<AStarMoveAction> actionStack = new Stack<AStarMoveAction>();
			while (etatActuel.getParent() != null) {
				actionStack.add(etatActuel.getActionFromParent());
				etatActuel = etatActuel.getParent();
			}

			while (!actionStack.empty()) {
				plan.add(actionStack.pop());
			}
		}

		return plan;
	}

	public static final void printDebut(Etat etat, GoalMove goal) {
		System.out.println("final  : f:" + etat.getF() + " | g:" + etat.getG()
				+ " | h:" + etat.getH() + " | "
				+ etat.getLocation().getPosition().divide(32.0f) + " | "
				+ goal.getDestination().getPosition().divide(32.0f));
	}
}
